//
// Created by xuwj in 2022 / 04.
//

#include <iostream>

#include "opencv2/core/core.hpp"

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"

#include "visual_frames_system.h"

using namespace std;
using namespace VNavAssistant;

const string file_path = "/home/xuwj/other_workspaces/visualNav_ws/src/vnav_assistant/configs/visual_frames_helper.yaml";

string tag_cam_frame;
string map_frame;

void TagOdomCallback(const nav_msgs::Odometry::ConstPtr &msg){

    static tf::TransformBroadcaster tf_pub;

    tf::Transform trans_cam_map;
    geometry_msgs::Pose pose_ = msg.get()->pose.pose;

    trans_cam_map.setRotation(tf::Quaternion(pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w));
    trans_cam_map.setOrigin(tf::Vector3(pose_.position.x, pose_.position.y, pose_.position.z));

//    ros::Time transform_expiration = ros::Time().fromSec(ros::Time::now().toSec() + trans_time_tolerance);
    tf::StampedTransform tf_stamped(trans_cam_map, ros::Time(0), tag_cam_frame, map_frame);
    tf_pub.sendTransform(tf_stamped);

}

int main(int argc, char **argv) {

    ros::init(argc, argv, "tag_check");
    ros::NodeHandle nh;

    //Load FrameID
    cv::FileStorage fSettings(file_path, CV_STORAGE_READ);
//    const string robot_base_frame = fSettings["RobotBaseFrame"];
//    const string camera_frame = fSettings["CameraFrame"];

    fSettings["MapFrame"]>>map_frame;
    fSettings["TagCamFrame"]>>tag_cam_frame;

    const string tag_topic = fSettings["AprilTagTopic"];
    const int tag_buff_size = fSettings["TagBuffSize"];


    ros::Subscriber sub_ = nh.subscribe<nav_msgs::Odometry>(tag_topic,tag_buff_size,TagOdomCallback);

    ros::spin();

    return 0;

}